/** \file emulation.c
 *
 * This is the mid level functions to control the blackfin emulation
 * features.
 *
 * Sections:
 *
 * \li REGISTER_ACCESS      register access
 * \li EMULATION_HANDLING   Emulation state handling
 * \li WATCHPOINT_UNIT      Watchpoint unit (hardware breaks)
 * \li MEMORY_ACCESS        memory access
 *
 * (c) 11/2004 Martin Strubel <hackfin@section5.ch>
 *
 * $Id: emulation.c 92 2007-02-10 15:51:16Z jiez $
 *
 *
 */

#if defined(__CYGWIN__) || defined(__MINGW32__)
#	include "windows.h"
#endif

#include <unistd.h>
#define __ADSPLPBLACKFIN__  // This define gets rid of warnings when including the
                            // following:
#include <defBF533.h>       // MMR i/o
#include "construct.h"      // Opcode construction

#include <stdio.h>
#include <stdlib.h>

#include "emu.h"
#include "error.h"


////////////////////////////////////////////////////////////////////////////
// PROTOS

#ifdef USE_FTDI
int delayed_read(CONTROLLER c, unsigned char *buf, unsigned int n);
#endif

////////////////////////////////////////////////////////////////////////////
// Auxiliary stuff

/* Endian handling -- TODO */

// XXX This is lazy. Fix it for proper network endians

#define SHORT_TO_BUF(x, buf)   *((unsigned short *) buf) = x
#define BUF_TO_SHORT(buf, x)   x = ((unsigned short *) buf)[0]
#define LONG_TO_BUF(x, buf)    *((unsigned long *) buf) = x
#define BUF_TO_LONG(buf, x)    x = ((unsigned long *) buf)[0]

/* Common instruction opcodes: */
static BFIN_REGISTER ins_emudat_r0;
static BFIN_REGISTER ins_r0_emudat;

#ifndef __WIN32
void m_sleep(int value)
{
	usleep(value * 1000);
}
#endif

void
init_ins(void)
{
	ins_emudat_r0 = cons_regmv(REG_EMUDAT, REG_R0);
	ins_r0_emudat = cons_regmv(REG_R0, REG_EMUDAT);
}


// XXX This function should die. Just to find out which order the bits
// come out of TDO...
//
unsigned long reverse(unsigned long data)
{
	int i;
	unsigned long rev = 0;

	for (i = 0; i < 32; i++) {
		rev <<= 1;
		rev |= data & 1;
		data >>= 1;
	}
	return rev;
}

BFIN_REGISTER get_dbgstat(Cpu *cpu)
{
	int error;
	BFIN_REGISTER out;

	jtagchain_select(cpu->controller);
	error = scanchain_select(SCAN_DBGSTAT);
	if (error < 0) {
		printf("Shift IR failed\n");
		HANDLE_ERROR(error);
	}

	jtag_goto_state(cpu->controller, s_jtag_shift_dr);
	error = shift_dr16_out_msb(cpu->controller, &out, UPDATE);
	if (error < 0) {
		printf("Shift DR failed\n");
		HANDLE_ERROR(error);
	}
	return out;
}

int set_dbgctl(Cpu *cpu, int exec)
{
	int error;
	jtag_goto_state(cpu->controller, s_jtag_shift_dr);
	error = shift_dr16_in_msb(cpu->controller, cpu->dbgctl, exec);
	return error;
}


////////////////////////////////////////////////////////////////////////////
// JTAG common routines

int
jtag_init(char * const *argv, Cpu **cpu)
{
	JTAG_CTRL ctrl;
	Cpu *c;
	int error;

	c = (Cpu *) malloc(sizeof(Cpu));
	if (!c) {
		fprintf(stderr, "Out of memory!\n");
		return ERR_MALLOC;
	}

	error = jtag_open(argv, &ctrl);
	if (error < 0) return error;

	// Init CPU:
	c->state = CPUSTATE_NORMAL;
	c->flags = 0;
	c->dbgctl = 0;
	c->controller = ctrl;

	*cpu = c;
	return ERR_NONE;
}

void
jtag_exit(Cpu *cpu)
{
	jtag_close(cpu->controller);
	free(cpu);
}

////////////////////////////////////////////////////////////////////////////
// REGISTER_ACCESS

static
int
g_allregs[] = {
	REG_R0, REG_R1, REG_R2, REG_R3, REG_R4, REG_R5, REG_R6, REG_R7, 
	REG_P0, REG_P1, REG_P2, REG_P3, REG_P4, REG_P5, REG_SP, REG_FP,
	REG_I0, REG_I1, REG_I2, REG_I3,
	REG_M0, REG_M1, REG_M2, REG_M3, 
	REG_B0, REG_B1, REG_B2, REG_B3,
	REG_L0, REG_L1, REG_L2, REG_L3, 
	REG_A0x, REG_A0w, REG_A1x, REG_A1w,
	REG_ASTAT,
	REG_RETS,
	REG_LC0, REG_LT0, REG_LB0,  REG_LC1, REG_LT1, REG_LB1,
	                  REG_CYCLES, REG_CYCLES2,

	REG_USP, REG_SEQSTAT, REG_SYSCFG,
	                  REG_RETI, REG_RETX, REG_RETN, REG_RETE, REG_EMUDAT,
  
	-1 // terminator
};

int save_registers(Cpu *cpu, BFIN_REGISTER *buf, int *regs)
{
	BFIN_REGISTER value;
	int error=0;  // PSW

	int *r = regs;
	while (*r >= 0) {
		jtag_queue(cpu->controller, 1);
		scanchain_select(SCAN_EMUIR);
		scanchain_shiftin(cons_regmv(REG_EMUDAT, *r), EXEC);
		scanchain_select(SCAN_EMUDAT);
		jtag_flush(cpu->controller);
		jtag_queue(cpu->controller, 0);
		error = scanchain_shiftout(&value, UPDATE);
		if (error < 0) break;
		*buf++ = value;

		r++;
	}
	scanchain_shiftin(INS_NOP, UPDATE);
	return error;
}

int restore_registers(Cpu *cpu, BFIN_REGISTER *buf, int *regs)
{
	jtag_queue(cpu->controller, 1);
	int *r = regs;
	while (*r >= 0) {
		scanchain_select(SCAN_EMUDAT);
		scanchain_shiftin(*buf++, UPDATE);
		scanchain_select(SCAN_EMUIR);
		scanchain_shiftin(cons_regmv(*r, REG_EMUDAT), EXEC);
		r++;
	}
	scanchain_shiftin(INS_NOP, UPDATE);
	jtag_flush(cpu->controller);
	jtag_queue(cpu->controller, 0);
	return 0;
}

BFIN_REGISTER
get_register(int regno)
{
	BFIN_REGISTER data;

	scanchain_select(SCAN_EMUIR);
	// send "EMUDAT = <reg>":
	scanchain_shiftin(cons_regmv(REG_EMUDAT, regno), EXEC);
	scanchain_shiftin(INS_NOP, UPDATE);
	scanchain_select(SCAN_EMUDAT);
	scanchain_shiftout(&data, UPDATE);   // read EMUDAT register via JTAG

	return data;
}

void
set_register(int regno, BFIN_REGISTER value)
{	
	scanchain_select(SCAN_EMUDAT);
	scanchain_shiftin(value, UPDATE);
	scanchain_select(SCAN_EMUIR);
	scanchain_shiftin(cons_regmv(regno, REG_EMUDAT), EXEC);//"<reg>=EMUDAT"
	scanchain_shiftin(INS_NOP, UPDATE);
}

BFIN_REGISTER
get_cpuregister(Cpu *cpu, int regno)
{
	BFIN_REGISTER data;

	jtag_queue(cpu->controller, 1);
	scanchain_select(SCAN_EMUIR);
	// send "EMUDAT = <reg>":
	scanchain_shiftin(cons_regmv(REG_EMUDAT, regno), EXEC);
	scanchain_shiftin(INS_NOP, UPDATE);
	scanchain_select(SCAN_EMUDAT);
	jtag_flush(cpu->controller);
	jtag_queue(cpu->controller, 0);
	scanchain_shiftout(&data, UPDATE);    // read EMUDAT register via JTAG

	return data;
}

void
set_cpuregister(CPU cpu, int regno, BFIN_REGISTER value)
{
	return set_register(regno, value);
}


////////////////////////////////////////////////////////////////////////////
// EMULATION_HANDLING

int
emulation_init(Cpu *cpu)
{
	jtagchain_select(cpu->controller);
	// Initialize common instructions:
	init_ins();

	jtag_reset(cpu->controller, 40);

	jtag_goto_state(cpu->controller, s_jtag_run_test_idle);
	get_idcode(cpu, &cpu->id);
	// Here, we have to determine the proper MDMA MMR base, depending
	// on the CPU type.
	switch (cpu->id & 0x0fffffff) {
		case IDCODE_BF533:
			cpu->mdma0_base = MDMA_D0_NEXT_DESC_PTR;
			break;
		case IDCODE_BF537: 
			cpu->mdma0_base = MDMA_D0_NEXT_DESC_PTR + 0x100;
			break;
		default:
			printf("Unknown CPU id, aborting\n");
			return ERR_UNSUPPORTED;
	}
	jtag_goto_state(cpu->controller, s_jtag_run_test_idle);

	scanchain_select(SCAN_DBGCTL);

	cpu->dbgctl = DBGCTL_EMPWR;       // enable Emulator power
	set_dbgctl(cpu, UPDATE);
	SLEEP(10);

	cpu->dbgctl |= DBGCTL_EMFEN;      // enable emulation
	set_dbgctl(cpu, UPDATE);

	cpu->dbgctl |= DBGCTL_EMPEN;      // enable Emulator pin
	set_dbgctl(cpu, UPDATE);

	emulation_config(cpu, EMUIR_SIZE, EMUIR_SIZE_32);
	emulation_config(cpu, EMUDAT_SIZE, EMUDAT_SIZE_32);

	cpu->state = CPUSTATE_NORMAL;
	return ERR_NONE;
}

void
emulation_exit(Cpu *cpu)
{
	jtagchain_select(cpu->controller);
	scanchain_select(SCAN_DBGCTL);

	cpu->dbgctl = 0;                  // Turn off emulator power, etc.
	set_dbgctl(cpu, EXEC);
}

void
emulation_enter(Cpu *cpu)
{
	if (cpu->state == CPUSTATE_EMULATION)
		return;
	if (cpu->state != CPUSTATE_NORMAL) {
		printf("emulation_enter(): CPU was not in normal state\n");
	}
	DEB(printf("EMULATION ENTER\n"));
	jtagchain_select(cpu->controller);

	jtag_queue(cpu->controller, 1);
	scanchain_select(SCAN_EMUIR);
	scanchain_shiftin(INS_NOP, UPDATE);   // send "NOP"

	DEB(printf("Set EMEEN\n"));
	scanchain_select(SCAN_DBGCTL);
	cpu->dbgctl |= DBGCTL_EMEEN | DBGCTL_WAKEUP; // trigger emulation event
	set_dbgctl(cpu, EXEC);

	cpu->state = CPUSTATE_EMULATION;
}

void
emulation_leave(Cpu *cpu)
{
	if (cpu->state != CPUSTATE_EMULATION) {
		printf("emulation_leave(): CPU was not in emulation state\n");
	}

	DEB(printf("EMULATION LEAVE\n"));

	jtagchain_select(cpu->controller);

	scanchain_select(SCAN_EMUIR);
	scanchain_shiftin(INS_RTE, UPDATE);

	scanchain_select(SCAN_DBGCTL);
	// assert emuin low
	cpu->dbgctl &= ~(DBGCTL_EMEEN | DBGCTL_ESSTEP | DBGCTL_WAKEUP);
	set_dbgctl(cpu, EXEC); // and execute RTE

	cpu->state = CPUSTATE_NORMAL;
}

void
emulation_enter_singlestep(Cpu *cpu)
{
	if (cpu->state != CPUSTATE_EMULATION) {
		printf("emulation_enter_singlestep(): not in emulation state, aborting\n");
		return;
	}
	emulation_config(cpu, EMU_STEP, 1);

	scanchain_select(SCAN_DBGCTL);
	cpu->dbgctl &= ~DBGCTL_EMEEN;   // assert emuin low
	scanchain_shiftin(cpu->dbgctl, UPDATE);

	cpu->state = CPUSTATE_STEP;
}

void
emulation_go(void)
{
	scanchain_select(SCAN_EMUIR);
	scanchain_shiftin(INS_RTE, EXEC);
}

void
emulation_leave_singlestep(Cpu *cpu)
{
	if (cpu->state != CPUSTATE_STEP) {
		printf("Warning: CPU was not in single step state, aborting\n");
		cpu->state = CPUSTATE_EMULATION;
		return;
	}

	emulation_config(cpu, EMU_STEP, 0);
	cpu->state = CPUSTATE_EMULATION;
}

void
emulation_flush(Cpu *cpu, ADDR addr, int n)
{
	BFIN_REGISTER p0;
	OPCODE32 ld0, ld1;

	if (n <= 0)
		return;

	jtagchain_select(cpu->controller);

	p0 = get_register(REG_P0);

	scanchain_select(SCAN_EMUIR);
	scanchain_shiftin(cons_ldimm(REG_P0, 0, addr), EXEC);
	scanchain_shiftin(cons_ldimm(REG_P0, 1, addr), EXEC);
	while (n--) {
		scanchain_shiftin(INS_IFLUSH_P0, EXEC);
	}
	scanchain_shiftin(INS_NOP, EXEC);

	set_register(REG_P0, p0);
}

int
emulation_config(Cpu *cpu, int mode, int value)
{
	jtagchain_select(cpu->controller);
	scanchain_select(SCAN_DBGCTL);

	switch (mode) {
		case EMUIR_SIZE:
			cpu->dbgctl &= ~DBGCTL_EMUIRSZ;
			cpu->dbgctl |= value;
			break;
		case EMUDAT_SIZE:
			cpu->dbgctl &= ~DBGCTL_EMUDATSZ;
			cpu->dbgctl |= value;
			break;
		case EMULOOP_SIZE:
			if (value == 2) {
				cpu->dbgctl |= DBGCTL_EMUIRLPSZ;
			} else {
				cpu->dbgctl &= ~DBGCTL_EMUIRLPSZ;
			}
			break;
		case EMU_STEP:
			if (value) {
				cpu->dbgctl |= DBGCTL_ESSTEP;
			} else {
				cpu->dbgctl &= ~DBGCTL_ESSTEP;
			}
			break;
		default:
			return -1;
	}

	set_dbgctl(cpu, UPDATE);
	return 0;
}


CpuState
emulation_state(Cpu *cpu)
{
	return cpu->state;
}

////////////////////////////////////////////////////////////////////////////
// WATCHPOINT_UNIT

void
mem_setbase(ADDR base)
{
	scanchain_shiftin(cons_ldimm(REG_P0, 0, base), EXEC);
	scanchain_shiftin(cons_ldimm(REG_P0, 1, base), EXEC);
}


void
mem_writeword(unsigned short offset, unsigned long data, int size)
{
	scanchain_shiftin(cons_ldimm(REG_R0, 0, data), EXEC);
	if (size == LDST_32) {
		scanchain_shiftin(cons_ldimm(REG_R0, 1, data), EXEC);
	}
	scanchain_shiftin(cons_stidxi(REG_P0, REG_R0, offset, size), EXEC);
}

int
wpu_init(Cpu *cpu, int enable)
{
	BFIN_REGISTER p0, r0;

	jtagchain_select(cpu->controller);

	if (!(get_dbgstat(cpu) & DBGSTAT_EMUREADY)) {
		printf("Error: Can not init WPU if not in emulation\n");
		return 0;
	}

	// save registers
	p0 = get_register(REG_P0);
	r0 = get_register(REG_R0);

	scanchain_select(SCAN_EMUIR);

	// Load base address of Watchpoint Unit MMR registers
	mem_setbase(WPIACTL);

	if (enable) {
		cpu->wpiactl = WPPWR;
		cpu->wpdactl = WPDSRC0(3) | WPDSRC1(3);
	} else {
		cpu->wpiactl = 
		cpu->wpdactl = 0;
	}

	mem_writeword(0, cpu->wpiactl,  LDST_32); // turn on/off WP unit
	SLEEP(1);
	cpu->wpiactl |= EMUSW0 | EMUSW1 | EMUSW2 | EMUSW3 | EMUSW4 | EMUSW5;
	mem_writeword(0, cpu->wpiactl, LDST_32); // configure EMUSWx

	// Data address watchpoint unit:
	// DAG0 and DAG1 as source, range check enable
	mem_writeword(WPDACTL - WPIACTL, cpu->wpdactl, LDST_32);

	mem_writeword(WPSTAT - WPIACTL, 0, LDST_32); // clear WPSTAT

	// restore registers
	set_register(REG_P0, p0);
	set_register(REG_R0, r0);

	return 1;
}

static
int wpuiactl_flags[6] = {
	WPIAEN0,
	WPIAEN1,
	WPIAEN2,
	WPIAEN3,
	WPIAEN4,
	WPIAEN5
};

#define XS_ALL  WPDACC0(WPDACC_R | WPDACC_W) | WPDACC1(WPDACC_R | WPDACC_W)
#define ENABLE01    WPDAEN0 | WPDAEN1 | WPDREN01

// Flags for Watchpoint data range
// First flags are cleared, second are set
static
int wpudactl_flags_r[4][2] = {
	{ ENABLE01, 0}, // WP_DISABLE
	{ XS_ALL, ENABLE01 | WPDACC0(WPDACC_R) | WPDACC1(WPDACC_R)}, // WP_READ
	{ XS_ALL, ENABLE01 | WPDACC0(WPDACC_W) | WPDACC1(WPDACC_W)}, // WP_WRITE
	{ 0, ENABLE01 | XS_ALL }  // WP_ALL
};


static
int wpudactl_flags[4][2] = {
	{ WPDAEN0, 0}, // WP_DISABLE
	{ XS_ALL, WPDAEN0 | WPDACC0(WPDACC_R) }, // WP_READ
	{ XS_ALL, WPDAEN0 | WPDACC0(WPDACC_W) }, // WP_WRITE
	{ 0, WPDAEN0 | WPDACC0(WPDACC_R | WPDACC_W) }  // WP_ALL
};


/* Set hardware breakpoint with number 'no' at address 'addr'
 */

int
wpu_set_hbreak(Cpu *cpu, int n, ADDR addr, int enable)
{
	BFIN_REGISTER p0, r0;

	jtagchain_select(cpu->controller);

	// save registers
	p0 = get_register(REG_P0);
	r0 = get_register(REG_R0);

	scanchain_select(SCAN_EMUIR);

	// Load base address of Watchpoint Unit MMR registers
	mem_setbase(WPIACTL);

	if (enable) {
		DEB(printf("--- Set HBREAK %d at %08lx\n", n, addr));
		cpu->wpiactl |= wpuiactl_flags[n];
		mem_writeword(WPIA0 + 4 * n - WPIACTL, addr, LDST_32); // set address
	} else {
		DEB(printf("--- Remove HBREAK %d at %08lx\n", n, addr));
		cpu->wpiactl &= ~wpuiactl_flags[n];
	}

	mem_writeword(0, cpu->wpiactl, LDST_32); // configure

	// restore registers
	set_register(REG_P0, p0);
	set_register(REG_R0, r0);

	return 1;
}

int
wpu_set_watch(Cpu *cpu, int n, ADDR addr, int mode)
{
	BFIN_REGISTER p0, r0;

	jtagchain_select(cpu->controller);

	// save registers
	p0 = get_register(REG_P0);
	r0 = get_register(REG_R0);

	scanchain_select(SCAN_EMUIR);

	// Set appropriate mode flags
	if (n > 4) {
		cpu->wpdactl &= ~(wpudactl_flags_r[mode][0]);
		cpu->wpdactl |= wpudactl_flags_r[mode][1];
	} else {
		cpu->wpdactl &= ~(wpudactl_flags[mode][0]);
		cpu->wpdactl |= wpudactl_flags[mode][1];
	}

	// Load base address of Watchpoint Unit MMR registers
	mem_setbase(WPDACTL);

	if (mode) {
		mem_writeword(WPDA0 - WPDACTL, addr, LDST_32);
		mem_writeword(WPDA1 - WPDACTL, addr + n, LDST_32);
	}

	mem_writeword(0, cpu->wpdactl, LDST_32); // configure
	printf("Write wpdactl<  %08lx\n", cpu->wpdactl);

	// restore registers
	set_register(REG_P0, p0);
	set_register(REG_R0, r0);
	
	return 1;
}

int
wpu_cause(Cpu *cpu)
{
	unsigned long r;

	jtagchain_select(cpu->controller);

	get_memory_word(WPSTAT, &r, LDST_32);
	set_memory_word(WPSTAT, r, LDST_32); // Any write access clears WPSTAT
	DEB(printf("WPSTAT: %08lx\n", r));
	return r;
}

////////////////////////////////////////////////////////////////////////////
// MEMORY_ACCESS


/* DMA transfer Memory <-> Memory
 *
 * DMA transfers: maximum 64 kB
 *
 */

int
init_dma(Cpu *cpu, ADDR dst, ADDR src, unsigned short count)
{
	ADDR offset, base;
	BFIN_REGISTER r;
	BFIN_REGISTER p0, r0;

	jtagchain_select(cpu->controller);

	base = MDMA_D0_NEXT_DESC_PTR;

	// save registers
	p0 = get_register(REG_P0);
	r0 = get_register(REG_R0);

	scanchain_select(SCAN_EMUIR);

	// Load base address of DMA MMR registers
	mem_setbase(cpu->mdma0_base);

	offset = MDMA_D0_IRQ_STATUS - base;
	scanchain_shiftin(cons_ldidxi(REG_P0, REG_R0, offset, LDST_16), EXEC);
	scanchain_shiftin(ins_emudat_r0, EXEC);
	scanchain_select(SCAN_EMUDAT);
	// TODO: check for currently active DMA
	scanchain_shiftout(&r, UPDATE);
	if (r & DMA_ERR) {
		printf("DMA ERROR!\n");
		return -1;
	}

	scanchain_select(SCAN_EMUIR);
	
	jtag_queue(cpu->controller, 1);
	//
	// Configure source DMA
	// Load source address:
	
	mem_writeword(MDMA_S0_START_ADDR - base, src, LDST_32);

	// Configure X count
	mem_writeword(MDMA_S0_X_COUNT - base, count, LDST_16);

	// Configure X modify
	mem_writeword(MDMA_S0_X_MODIFY - base, 1, LDST_16);

	// Configure word size 8 and go
	mem_writeword(MDMA_S0_CONFIG - base, DI_EN | WDSIZE_8 | DMAEN, LDST_16);

	// Configure dest DMA
	base = MDMA_D0_NEXT_DESC_PTR;
	mem_setbase(cpu->mdma0_base);

	// Load source address:
	mem_writeword(MDMA_D0_START_ADDR - base, dst, LDST_32);

	// Configure X count
	mem_writeword(MDMA_D0_X_COUNT - base, count, LDST_16);

	// Configure X modify
	mem_writeword(MDMA_D0_X_MODIFY - base, 1, LDST_16);
	
	// Configure word size 8 and go
	mem_writeword(MDMA_D0_CONFIG - base,
			DI_EN | WNR | WDSIZE_8 | DMAEN, LDST_16);

	jtag_flush(cpu->controller);
	jtag_queue(cpu->controller, 0);

	// Now, poll status
	offset = MDMA_S0_IRQ_STATUS - base;
	int retries = 0;
	do {
		jtag_queue(cpu->controller, 1);
		scanchain_select(SCAN_EMUIR);
		scanchain_shiftin(cons_ldidxi(REG_P0, REG_R0, offset, LDST_16), EXEC);
		scanchain_shiftin(ins_emudat_r0, EXEC);
		scanchain_select(SCAN_EMUDAT);
		jtag_flush(cpu->controller);
		jtag_queue(cpu->controller, 0);
		// DEB(printf("DMA running..\n"));
		scanchain_shiftout(&r, UPDATE);
		// XXX: Handle proper timeouts here.
		if (retries > 20000) {
			printf("DMA hung!\n");
			break;
		}
		retries++;
	} while (!(r & (DMA_DONE | DMA_ERR)));

	if (r & DMA_ERR) {
		printf("DMA error\n");
		return -1;
	}
	
	// restore registers
	set_register(REG_P0, p0);
	set_register(REG_R0, r0);

	return jtag_flush(cpu->controller);
}

/* DAG capable memory read access
 * TODO: Data transfer could be much faster when we would be using
 * full words instead of bytes..
 */

int
get_datamemory(Cpu *cpu, ADDR addr, unsigned long count, unsigned char *buf)
{
	BFIN_REGISTER context[2];
	BFIN_REGISTER r;
	static int regs[] = {REG_P0, REG_R0, -1} ;

	OPCODE32 ld;

	int i;

	if (count == 0) return 0;

	jtagchain_select(cpu->controller);

	// Construct r0 = b [p0++] (z);
	ld = cons_ldst(REG_P0, REG_R0, LDST_8, LDST_READ);

	// save p0 and r0
	save_registers(cpu, context, regs);

	jtag_queue(cpu->controller, 1);
	scanchain_select(SCAN_EMUIR);

	mem_setbase(addr);

	emulation_config(cpu, EMULOOP_SIZE, 2);

	scanchain_select(SCAN_EMUIR);
	// We execute immediately:  r0 = b [p0++] (z); emudat = r0;
	scanchain_shift_twoins(ld, ins_emudat_r0, EXEC);

	scanchain_select(SCAN_EMUDAT);
	i = count - 1;

	jtag_flush(cpu->controller);
#ifdef USE_FTDI
	delayed_read(cpu->controller, buf, i);
	buf += i;
#else
	while (i--) {
		scanchain_shiftout(&r, EXEC);
		*buf++ = r;
	}
#endif

	scanchain_select(SCAN_EMUIR);
	scanchain_shift_twoins(INS_NOP, INS_NOP, UPDATE);
	scanchain_select(SCAN_EMUDAT);
	scanchain_shiftout(&r, EXEC);
	*buf++ = r;

	emulation_config(cpu, EMULOOP_SIZE, 1);


	restore_registers(cpu, context, regs);
	jtag_flush(cpu->controller);
	jtag_queue(cpu->controller, 0);

	return 0;
}

/* DAG capable memory write access */

int
set_datamemory(Cpu *cpu, ADDR addr, unsigned long size,
		const unsigned char *buf)
{
	BFIN_REGISTER context[2];
	static int regs[] = {REG_P0, REG_R0, -1} ;
	int sizecode = LDST_8;
	BFIN_REGISTER value;
	unsigned long count = size;

	OPCODE32 st;
	if (count == 0) return 0;

	// if we have even addresses and even count, we can 
	// use word size transfers instead of byte wise.
	if (addr % 2 == 0) {
		switch (count % 4) {
			case 0:
				sizecode = LDST_32;
				count /= 4;
				break;
			case 2:
				sizecode = LDST_16;
				count /= 2;
				break;
			default:
				sizecode = LDST_8;
				break;
		}
	}

	st = cons_ldst(REG_P0, REG_R0, sizecode, LDST_WRITE);

	// save p0 and r0
	save_registers(cpu, context, regs);

	jtag_queue(cpu->controller, 1);

	scanchain_select(SCAN_EMUIR);

	mem_setbase(addr);

	emulation_config(cpu, EMULOOP_SIZE, 2);

	scanchain_select(SCAN_EMUIR);
	scanchain_shift_twoins(ins_r0_emudat, st, UPDATE);

#ifdef DEBUG
	TIMEVAL t0, t1;
	gettime(&t0);
#endif
	scanchain_select(SCAN_EMUDAT);


	switch (sizecode) {
		case LDST_8:
			while (count--) {
				// Every time we do 'full' shift and enter Run-Test/Idle,
				// the instruction loop is executed
				scanchain_shiftin(*buf++, EXEC);
			}
			break;
		case LDST_16:
			while (count--) {
				value = buf[0] | (buf[1] << 8);
				scanchain_shiftin(value, EXEC);
				buf += 2;
			}
			break;
		case LDST_32:
			while (count--) {
				value =
					buf[0] | (buf[1] << 8) | (buf[2] << 16) | (buf[3] << 24);
				// printf("val: %08lx\n", value);
				scanchain_shiftin(value, EXEC);
				buf += 4;
			}
			break;
	}

	scanchain_select(SCAN_EMUIR);
	scanchain_shift_twoins(INS_NOP, INS_NOP, EXEC);
	emulation_config(cpu, EMULOOP_SIZE, 1);

#ifdef DEBUG
	gettime(&t1);
	float t = took_time(&t0, &t1);
	printf("took %f\n", (float) t);
	printf("rate: %f kB/s\n", (float) size / 1000.0 / t);
#endif
	// restore r0, p0
	restore_registers(cpu, context, regs);

	jtag_flush(cpu->controller);
	jtag_queue(cpu->controller, 0);

	return 0;
}


int
get_memory_word(ADDR addr, unsigned long *word, int size)
{
	int error;
	BFIN_REGISTER p0, r0;
	OPCODE32 ld;

	ld = cons_ldst(REG_P0, REG_R0, size, LDST_READ);

	/* save p0 and r0 */
	p0 = get_register(REG_P0);
	r0 = get_register(REG_R0);

	scanchain_select(SCAN_EMUIR);
	/* Construct p0.l = lo(addr) */
	scanchain_shiftin(cons_ldimm(REG_P0, 0, addr), EXEC);
	/* Construct p0.h = hi(addr) */
	scanchain_shiftin(cons_ldimm(REG_P0, 1, addr), EXEC);

	scanchain_shiftin(ld, EXEC);
    scanchain_shiftin(ins_emudat_r0, EXEC);
	scanchain_select(SCAN_EMUDAT);
	error = scanchain_shiftout(word, UPDATE);

	scanchain_select(SCAN_EMUIR);
	scanchain_shiftin(INS_NOP, UPDATE);

	/* restore registers */
	set_register(REG_P0, p0);
	set_register(REG_R0, r0);
	return error;
}

int
set_memory_word(ADDR addr, const unsigned long value, int size)
{
	BFIN_REGISTER p0, r0;
	OPCODE32 st;

	st = cons_ldst(REG_P0, REG_R0, size, LDST_WRITE);

	/* save p0 and r0 */
	p0 = get_register(REG_P0);
	r0 = get_register(REG_R0);

	scanchain_select(SCAN_EMUIR);
	mem_setbase(addr);
	mem_writeword(0, value, size);

	/* restore registers */
	set_register(REG_P0, p0);
	set_register(REG_R0, r0);

	return 0;
}


/* Non-DAG capable memory access (L1 instruction SRAM) */
int
get_l1memory(Cpu *cpu, ADDR addr, unsigned long count, unsigned char *buf)
{
	int error;
	unsigned char tempbuf[count];
	// transfer code memory content into data memory
	if (count == 0) return 0;
	else if (count > 0xffff) return ERR_UNSUPPORTED;

	DEB(printf("Read L1 @%08lx[%lu]\n", addr, count));

	error = get_datamemory (cpu, DATA_BUF, count, tempbuf);
	if (error < 0) return error;

	error = init_dma(cpu, DATA_BUF, addr, count);
	if (error < 0) return error;

	error = get_datamemory(cpu, DATA_BUF, count, buf);
	if (error < 0) return error;

	error = set_datamemory (cpu, DATA_BUF, count, tempbuf);

	return error;
}

int
set_l1memory(Cpu *cpu, ADDR addr, unsigned long count, const unsigned char *buf)
{
	int error;
	unsigned char tempbuf[count];

	// First, write data to L1 data buffer, then transfer into L1 code via DMA
	if (count == 0) return 0;
	else if (count > 0xffff) return ERR_UNSUPPORTED;
	DEB(printf("Write L1 @%08lx[%lu]\n", addr, count));

	error = get_datamemory (cpu, DATA_BUF, count, tempbuf);
	if (error < 0) return error;

	error = set_datamemory(cpu, DATA_BUF, count, buf);
	if (error < 0) return error;

	error = init_dma(cpu, addr, DATA_BUF, count);
	if (error < 0) return error;

	error = set_datamemory (cpu, DATA_BUF, count, tempbuf);

	return error;
}

/* MMR register access */
int 
get_mmr(unsigned long addr, unsigned long count,
		unsigned char *buf)
{
	unsigned long r;
	int error;

	switch (count) {
		case 0: // we support this, because GDB seems to use it for probing..
			error = 0; 
			break;
		case 2:
			error = get_memory_word(addr, &r, LDST_16);
			SHORT_TO_BUF(r, buf);
			break;
		case 4:
			error = get_memory_word(addr, &r, LDST_32);
			LONG_TO_BUF(r, buf);
			break;
		default:
			error = ERR_UNSUPPORTED;
	}
	
	return error;
}

int
set_mmr(unsigned long addr, unsigned long count,
		const unsigned char *buf)
{
	unsigned long reg;
	int error;

	switch (count) {
		case 0: // we support this, because GDB seems to use it for probing..
			error = 0; 
			break;
		case 2:
			BUF_TO_SHORT(buf, reg);
			error = set_memory_word(addr, reg, LDST_16);
			break;
		case 4:
			BUF_TO_LONG(buf, reg);
			error = set_memory_word(addr, reg, LDST_32);
			break;
		default:
			error = ERR_UNSUPPORTED;
	}
	return error;
}



/* Memory access wrappers
 *
 * These have to do some address decoding, since memory is accessed
 * in different ways..
 */

#define ADDR_DECODE_AND_ACCESS(addr, which)                                \
	if ( (addr >= MEM_SDRAM && addr < MEM_L1_INSTR)                        \
	  || (addr >= MEM_L1_SCRATCH && addr < MEM_L1_SCRATCH_E) ) {           \
		return which##_datamemory(cpu, addr, count, buf);                  \
	} else                                                                 \
	if (addr >= MEM_L1_INSTR) {                                            \
		if (addr <= MEM_L1_INSTR_E) {                                      \
			return which##_l1memory(cpu, addr, count, buf);                \
		} else                                                             \
		if (addr >= MEM_MMR && (addr % 2) == 0) {                          \
			return which##_mmr(addr, count, buf);                          \
		}                                                                  \
	} 

int
get_memory(Cpu *cpu, ADDR addr, unsigned long count,
		unsigned char *buf)
{
	ADDR_DECODE_AND_ACCESS(addr, get)
	return -1;
}


int
set_memory(Cpu *cpu, ADDR addr, unsigned long count,
		const unsigned char *buf)
{
	ADDR_DECODE_AND_ACCESS(addr, set)
	return -1;
}

////////////////////////////////////////////////////////////////////////////
// RESETTING

void
sys_reset(Cpu *cpu)
{
	DEB(printf("System RESET\n"));

	jtagchain_select(cpu->controller);

	scanchain_select(SCAN_EMUIR);
	scanchain_shiftin(cons_ldimm(REG_P0, 0, SWRST), EXEC);
	scanchain_shiftin(cons_ldimm(REG_P0, 1, SWRST), EXEC);
	scanchain_shiftin(cons_ldimm(REG_R0, 0, 0x0007), EXEC);
	scanchain_shiftin(cons_ldst(REG_P0, REG_R0, LDST_16, LDST_WRITE), EXEC);
	scanchain_shiftin(INS_SSYNC, EXEC);
	scanchain_shiftin(cons_ldimm(REG_R0, 0, 0x0000), EXEC);
	scanchain_shiftin(cons_ldst(REG_P0, REG_R0, LDST_16, LDST_WRITE), EXEC);
	scanchain_shiftin(INS_SSYNC, EXEC);
}


void
core_reset(Cpu *cpu)
{
	int retries = 0;
	BFIN_REGISTER dbgstat;
	DEB(printf("Core RESET\n"));

	scanchain_select(SCAN_DBGCTL);
	cpu->dbgctl |= DBGCTL_SYSRST; scanchain_shiftin(cpu->dbgctl, EXEC);
	SLEEP(100);
	cpu->dbgctl &= ~DBGCTL_SYSRST; scanchain_shiftin(cpu->dbgctl, UPDATE);

	while (!((dbgstat = get_dbgstat(cpu)) & DBGSTAT_EMUREADY)) {
		SLEEP(100);
		retries++;
		if (retries > 10) {
			printf("Could not reset core\n");
			break;
		}
	}

	if (!((dbgstat = get_dbgstat(cpu)) & DBGSTAT_EMUREADY)) {
		fprintf(stderr, "Error: could not reset core\n");
	}

	DEB(printf("DBGSTAT: %08X PC: %08X\n", dbgstat, get_cpuregister(cpu, REG_RETE)));
}

// TODO: This function needs some more functionality (reset PC, etc.).

void
cpu_reset(Cpu *cpu, int mode)
{
	if (mode & 1)
		core_reset(cpu);
	if (mode & 2)
		sys_reset(cpu);
}

////////////////////////////////////////////////////////////////////////////
//
// DEVICE DETECT


static struct IdCode {
	unsigned long code;
	char  *idstring;
} g_ids[] = {
	{ 0x027a50cb, "BF-533" },
	{ 0x027c80cb, "BF-537" },
	{ 0 }
};


int get_idcode(Cpu *cpu, unsigned long *code)
{
	int error;
	BFIN_REGISTER value;
	//printf("Get IDCODE\n");
	jtagchain_select(cpu->controller);
	scanchain_select(SCAN_IDCODE);
	error = scanchain_shiftout(&value, UPDATE);
	*code = reverse(value);
	return error;
}

char *detectDevice(Cpu *cpu, unsigned int *rev)
{
	int error;
	BFIN_REGISTER code;

	error = get_idcode(cpu, &code);

	if (error < 0) {
		HANDLE_ERROR(error); return NULL;
	}

	// retrieve silicon revision:
	*rev = (code >> 28) & 0xf;

	printf("Idcode: %08lx\n", code);
	struct IdCode *walk = g_ids;

	code &= 0x0fffffff; // mask revision code
	while (walk->code) {
		if (code == walk->code) break;
		walk++;
	}
	return walk->idstring;
}

